// g2o - General Graph Optimization
// Copyright (C) 2012 R. Kümmerle
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
// * Redistributions of source code must retain the above copyright notice,
//   this list of conditions and the following disclaimer.
// * Redistributions in binary form must reproduce the above copyright
//   notice, this list of conditions and the following disclaimer in the
//   documentation and/or other materials provided with the distribution.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#pragma once
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <iostream>
#include "g2o/types/sba/vertex_se3_expmap.h"
#include "g2o/types/sba/edge_project_xyz2uv.h"
#include "g2o/types/sba/parameter_cameraparameters.h"

class G2O_TYPES_SBA_API MyCameraParameters : public g2o::BaseParameter<g2o::StereoCameraParameters>
{
public:
  MyCameraParameters() = default;
  MyCameraParameters(double focalLength,
                     const Eigen::Ref<const g2o::Vector2> &principlePoint,
                     double baseLine);

  [[nodiscard]] g2o::Vector2 cam_map(const g2o::Vector3 &trans_xyz) const;
  [[nodiscard]] g2o::Vector3 stereocam_uvu_map(const g2o::Vector3 &trans_xyz) const;

  void update() override;
};
MyCameraParameters::MyCameraParameters(
    double focalLength, const Eigen::Ref<const g2o::Vector2> &principlePoint,
    double baseLine)
{
  parameter_.focal_length = focalLength;
  parameter_.principle_point = principlePoint;
  parameter_.baseline = baseLine;
}

g2o::Vector2 MyCameraParameters::cam_map(const g2o::Vector3 &trans_xyz) const
{
  g2o::Vector2 res;
  res[0] = trans_xyz[0] / trans_xyz[2] * parameter_.focal_length + parameter_.principle_point[0];
  res[1] = trans_xyz[1] / trans_xyz[2] * parameter_.focal_length + parameter_.principle_point[1];
  return res;
}

g2o::Vector3 MyCameraParameters::stereocam_uvu_map(const g2o::Vector3 &trans_xyz) const
{
  g2o::Vector2 uv_left = cam_map(trans_xyz);
  double proj_x_right = (trans_xyz[0] - parameter_.baseline) / trans_xyz[2];
  double u_right =
      proj_x_right * parameter_.focal_length + parameter_.principle_point[0];
  return g2o::Vector3(uv_left[0], uv_left[1], u_right);
}

void MyCameraParameters::update() {}

class G2O_TYPES_SBA_API VertexSE3ExpmapOnlyTrans : public g2o::BaseVertex<6, g2o::SE3Quat>
{
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  // 新版G2O中只需要实现这个函数就可以了
  void oplusImpl(const g2o::VectorX::MapType &update) override
  {
    // g2o::SE3Quat up = g2o::SE3Quat::exp(update);
    // g2o::Vector7 est = estimate().toVector();
    // std::cout << "Delta T: " << up.to_homogeneous_matrix() << '\n';
    // std::cout << "before update: " << est.transpose() << '\n';
    setEstimate(g2o::SE3Quat::exp(update.head<kDimension>()) * estimate());
    // est = estimate().toVector();
    // std::cout << "after update: " << est.transpose() << '\n';
  };
};

/*自定义仅含有位置偏移的边*/
class G2O_TYPES_SBA_API EdgeSE3ProjectXYZOnlyTrans : public g2o::BaseBinaryEdge<2, g2o::Vector2, g2o::VertexPointXYZ, VertexSE3ExpmapOnlyTrans>
{
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
public:
  EdgeSE3ProjectXYZOnlyTrans()
  {
    resizeParameters(1);
    installParameter<MyCameraParameters>(0);
  };
  void computeError() override
  {
    const VertexSE3ExpmapOnlyTrans *v_cam_rt = vertexXnRaw<1>();
    const g2o::VertexPointXYZ *v_points_3d = vertexXnRaw<0>();
    auto cam = std::static_pointer_cast<MyCameraParameters>(parameter(0));
    error_ = measurement() - cam->cam_map(v_cam_rt->estimate().map(v_points_3d->estimate()));
  };

  std::shared_ptr<MyCameraParameters> _cam;
  void linearizeOplus() override
  {
    VertexSE3ExpmapOnlyTrans *vj = vertexXnRaw<1>();
    g2o::SE3Quat T(vj->estimate());
    g2o::VertexPointXYZ *vi = vertexXnRaw<0>();
    g2o::Vector3 xyz = vi->estimate();
    g2o::Vector3 xyz_trans = T.map(xyz);

    double x = xyz_trans[0];
    double y = xyz_trans[1];
    double z = xyz_trans[2];
    double z_2 = z * z;

    const g2o::StereoCameraParameters &cam =
        static_cast<MyCameraParameters *>(parameter(0).get())->param();

    Eigen::Matrix<double, 2, 3, Eigen::ColMajor> tmp;
    tmp(0, 0) = cam.focal_length;
    tmp(0, 1) = 0;
    tmp(0, 2) = -x / z * cam.focal_length;

    tmp(1, 0) = 0;
    tmp(1, 1) = cam.focal_length;
    tmp(1, 2) = -y / z * cam.focal_length;

    jacobianOplusXi_ = -1. / z * tmp * T.rotation().toRotationMatrix();
    jacobianOplusXi_.setZero();
    // 仅更新平移部分
    jacobianOplusXj_(0, 0) = x * y / z_2 * cam.focal_length;
    jacobianOplusXj_(0, 1) = -(1 + (x * x / z_2)) * cam.focal_length;
    jacobianOplusXj_(0, 2) = y / z * cam.focal_length;
    jacobianOplusXj_(0, 3) = -1. / z * cam.focal_length;
    jacobianOplusXj_(0, 4) = 0;
    jacobianOplusXj_(0, 5) = x / z_2 * cam.focal_length;

    jacobianOplusXj_(1, 0) = (1 + y * y / z_2) * cam.focal_length;
    jacobianOplusXj_(1, 1) = -x * y / z_2 * cam.focal_length;
    jacobianOplusXj_(1, 2) = -x / z * cam.focal_length;
    jacobianOplusXj_(1, 3) = 0;
    jacobianOplusXj_(1, 4) = -1. / z * cam.focal_length;
    jacobianOplusXj_(1, 5) = y / z_2 * cam.focal_length;
    // jacobianOplusXj_.block<2, 3>(0, 0).setZero();
  };
};
